
#include "PIT3.h"
#include "ChassisControl.h"
#include "Display.h"

void PIT3_Init()
{
    pit_ms_init(PIT_CH3, 10);
    interrupt_set_priority(PIT_IRQn, 3);
}

CarmMovement* SeqS = NULL;
uint32 nowstep = 0;
uint32 SeqsLenth = 0;

void AddRuningSequence(CarmMovement* SeqsData, uint32 lenth)
{
    if (SeqS) free(SeqS);

    SeqS = malloc(sizeof(CarmMovement) * lenth);
    memcpy(SeqS, SeqsData, sizeof(CarmMovement) * lenth);
    SeqsLenth = lenth;
    nowstep = 0;
}









void PIT3_handler()
{

    if (_Carmode == RuningMode)
    {
        //DisplayMenu();
    }
    /*
    if (nowstep < SeqsLenth && SeqS)
    {
        float r = sqrt((NowX - SeqS[nowstep].MoveX) * (NowX - SeqS[nowstep].MoveX) +
            (NowY - SeqS[nowstep].MoveY) * (NowY - SeqS[nowstep].MoveY));

        if (r < 0.005f)
        {
            nowstep++;
        }
        else
        {
            Xtarget = SeqS[nowstep].MoveX;
            Ytarget = SeqS[nowstep].MoveY;
            AngZTarget = SeqS[nowstep].MoveAng;
        }
    }
    else
    {

        if (SeqS)free(SeqS);
        SeqS = NULL;
    }
    */
}

